//Rotation 3D Scanner

#include "libfreenect.hpp"
#include <iostream>
#include <fstream>
#include <sstream>
#include <cmath>
#include <vector>
#include <string>
#include <ctime>

#if defined(__APPLE__)
#include <GLUT/glut.h>
#include <OpenGL/gl.h>
#include <OpenGL/glu.h>
#else
#include <GL/glut.h>
#include <GL/gl.h>
#include <GL/glu.h>
#endif


#define SUCCESS 0
#define NO_SUCCESS 1


typedef unsigned char uchar;
typedef unsigned short ushort;
typedef unsigned int  uint_16;

using namespace std;



class KinectController 
{
   public:

   virtual int loadImages() = 0; 
   virtual int loadLockImages(uchar** rgbLockPtr, ushort** depthLockPtr) = 0; 
   virtual int getRGB(uchar** rgbPtr) = 0;
   virtual int getDEPTH(ushort** depthPtr) = 0;
   virtual bool getDepthFlag() = 0;
   virtual int setDepthFlag() = 0;
   virtual int getDepthForGlut(ushort** depthShowPtr) = 0;

};


class MyKinectController : public KinectController, public Freenect::FreenectDevice 
{
   private:	

   uchar *video_buffer;
   uchar *rgb_save_buffer;
   ushort *depth_buffer;
   ushort *depth_save_buffer;
   uchar *rgb_save_lock_buffer;
   ushort *depth_save_lock_buffer;
   ushort *depth_show_buffer;
   bool depth_flag;

   public:

   MyKinectController(freenect_context *_ctx, int _index) : Freenect::FreenectDevice(_ctx, _index) 
   {
      depth_flag = false;
      video_buffer = new uchar[640*480*3];
      rgb_save_buffer = new uchar[640*480*3];
      rgb_save_lock_buffer = new uchar[640*480*3];

      depth_buffer = new ushort[640*480];
      depth_save_buffer = new ushort[640*480];
      depth_save_lock_buffer = new ushort[640*480];
      
      depth_show_buffer = new ushort[640*480];

   }

   ~MyKinectController()
   {
      delete []video_buffer;
      delete []rgb_save_buffer;
      delete []depth_buffer;
      delete []depth_save_buffer;
      delete []rgb_save_lock_buffer;
      delete []depth_save_lock_buffer;
      delete []depth_show_buffer;
   }


   // Kinect RGB Stream auf video_buffer kopieren
   void VideoCallback(void* _rgb, uint32_t timestamp) 
   {
      uchar* rgb = static_cast<uchar*>(_rgb);
      memcpy(video_buffer, rgb, 640*480*3);
   }


   // Kinect Depth Stream auf depth_buffer kopieren
   void DepthCallback(void* _depth, uint32_t timestamp) 
   {
      ushort* depth = static_cast<ushort*>(_depth);
      memcpy(depth_buffer, depth, sizeof(ushort)*640*480);
      depth_flag = true;
   }


   int loadImages()
   {	
      memcpy(rgb_save_buffer, video_buffer, 640*480*3);
      memcpy(depth_save_buffer, depth_buffer, sizeof(ushort)*640*480);

      for (int i=0; i <= 640*480-1; i++) 
      {
         if (depth_save_buffer[i] == 2047) 
         {
            depth_show_buffer[i] = 0;
         }
         else 
         {
            depth_show_buffer[i] = 32 * depth_save_buffer[i];
         }

      }
      
      return(SUCCESS);
   }
   
   int loadLockImages(uchar** rgbLockPtr, ushort** depthLockPtr)
   {	
      memcpy(rgb_save_lock_buffer, video_buffer, 640*480*3);
      memcpy(depth_save_lock_buffer, depth_show_buffer, sizeof(ushort)*640*480);
      
      *rgbLockPtr = rgb_save_lock_buffer;
      *depthLockPtr = depth_save_lock_buffer;
      
      return(SUCCESS);
   }


   // Pointer auf rgb_save_buffer an Mastercontroller
   int getRGB(uchar** rgbPtr)
   {
      *rgbPtr = rgb_save_buffer;
      return(SUCCESS);
   }


   // Pointer auf depth_save_buffer an Mastercontroller
   int getDEPTH(ushort** depthPtr)
   {
      *depthPtr = depth_save_buffer;
      return(SUCCESS);
   }	
   
   int getDepthForGlut(ushort** depthShowPtr)
   {
      *depthShowPtr = depth_show_buffer;
      return(SUCCESS);
   }
   
   bool getDepthFlag()
   {
      return(depth_flag);
   }


   int setDepthFlag()
   {
      depth_flag = false;
      return(SUCCESS);
   }
};

double freenect_angle(0);
int got_frames(0),window(0);
int g_argc;
char **g_argv;
int a=1;
clock_t timed;

void outerGlutDisplayFunc();
void outerglutIdleFunc();
void outerGlutReshapeFunc(int Width, int Height);
void outerGlutKeyboardFunc(unsigned char key, int x, int y);



class MasterController
{
   private:

   int i , e, t;
   bool startFlag;
   //ushort* depth;
   //uchar* rgb;
   GLuint gl_depth_tex;
   GLuint gl_rgb_tex;
   GLuint gl_depth_lock_tex;
   GLuint gl_rgb_lock_tex;


   Freenect::Freenect freenect;


   public:

   uchar* rgbPtr;
   uchar* rgbLockPtr;
   ushort* depthShowPtr;
   ushort* depthPtr;
   ushort* depthLockPtr;
   bool lockFlag;
   MyKinectController* kinectController;
   
   
   void init()
   {
      lockFlag = false;
      t = 1;
      rgbLockPtr = 0;
      depthLockPtr = 0;
      startFlag = false;
      gl_rgb_lock_tex = 0;
      gl_depth_lock_tex = 0;

      
      kinectController = &freenect.createDevice<MyKinectController>(0);
      i=1;
      
      glutInit(&g_argc, g_argv);
      
      glutInitDisplayMode(GLUT_RGBA | GLUT_DOUBLE | GLUT_ALPHA | GLUT_DEPTH);
      glutInitWindowSize(1024, 768);
      glutInitWindowPosition(0, 0);
      
      window = glutCreateWindow("LibFreenect");
      
      glutDisplayFunc(outerGlutDisplayFunc);
      glutIdleFunc(outerglutIdleFunc);
      glutReshapeFunc(outerGlutReshapeFunc);
      glutKeyboardFunc(outerGlutKeyboardFunc);
      
      InitGL(1024, 768);
   }
   
   MasterController()
   {
  
   }

   
   ~MasterController()
   {
      delete rgbPtr;
      delete depthPtr;

      delete rgbLockPtr;
      delete depthLockPtr;
      delete depthShowPtr;
      //delete kinectController;
   }

   
   int writeToBinaryFile(uchar **rgbPtr, ushort **depthPtr, int i)
   {
      char rgbFileName[10];
      char depthFileName[10];
      char imageFileName[10];

      FILE *rgbRawFile;
      FILE *depthRawFile;
      FILE *imageFile;
      
      if (i<10) {
         sprintf(rgbFileName,"rgb_0%i.bin",i);
         sprintf(depthFileName,"depth_0%i.bin",i);
         sprintf(imageFileName,"image_0%i.ppm",i);
      }
      else 
      {
         sprintf(rgbFileName,"rgb_%i.bin",i);
         sprintf(depthFileName,"depth_%i.bin",i);
         sprintf(imageFileName,"image_%i.ppm",i);
      }
      
      rgbRawFile = fopen(rgbFileName, "wb");
      depthRawFile = fopen(depthFileName, "wb");
      imageFile = fopen(imageFileName, "wb");
      
      fprintf(imageFile, "P6 640 480 255\n");
      fwrite(*rgbPtr, sizeof(uchar), 640*480*3, imageFile);

      fwrite(*rgbPtr, sizeof(uchar), 640*480*3, rgbRawFile);
      fwrite(*depthPtr, sizeof(ushort), 640*480, depthRawFile);
      
      fclose(imageFile);
      fclose(rgbRawFile);
      fclose(depthRawFile);
      
      cout << "Files #"<< i << " written\n" <<endl;
      return(SUCCESS);
   }
   
   void setStartFlagOn()
   {  
      cout << "begin with scan" << endl;
      startFlag = true;
      timed = clock();
   }
   
   int start() 
   {
      kinectController->startVideo();
      kinectController->startDepth();
      
      glutMainLoop();
      
      return(SUCCESS);
   }
   
   bool checkTime()
   {
      if((clock() - timed) >= 9.1*CLOCKS_PER_SEC) //9.1 cause of platform problem with time.h
      {
         timed = clock();
         return(true);
      }
      return(false);
   }
   
   void draw()
   {	
      //kinectController->updateState();
      //printf("\r demanded tilt angle: %+4.2f device tilt angle: %+4.2f", freenect_angle, kinectController->getState().getTiltDegs());

      fflush(stdout);
      
      e = kinectController->loadImages();
      e = kinectController->getRGB(&rgbPtr);
      e = kinectController->getDEPTH(&depthPtr);
      e = kinectController->getDepthForGlut(&depthShowPtr);
      
      got_frames = 0;
      
      glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
      glLoadIdentity();
      
      glEnable(GL_TEXTURE_2D);
      
      glBindTexture(GL_TEXTURE_2D, gl_depth_tex);
      glTexImage2D(GL_TEXTURE_2D, 0, 1, 640, 480, 0, GL_LUMINANCE, GL_UNSIGNED_SHORT, &depthShowPtr[0]);	
      
      glBegin(GL_TRIANGLE_FAN);
      glColor4f(255.0f, 255.0f, 255.0f, 255.0f);
      glTexCoord2f(0, 0); glVertex3f(0,0,0);
      glTexCoord2f(1, 0); glVertex3f(640,0,0);
      glTexCoord2f(1, 1); glVertex3f(640,480,0);
      glTexCoord2f(0, 1); glVertex3f(0,480,0);
      glEnd();
      
      glBindTexture(GL_TEXTURE_2D, gl_rgb_tex);
      glTexImage2D(GL_TEXTURE_2D, 0, 3, 640, 480, 0, GL_RGB, GL_UNSIGNED_BYTE, &rgbPtr[0]);
      
      glBegin(GL_TRIANGLE_FAN);
      glColor4f(255.0f, 255.0f, 255.0f, 255.0f);
      glTexCoord2f(0, 0); glVertex3f(640,0,0);
      glTexCoord2f(1, 0); glVertex3f(1280,0,0);
      glTexCoord2f(1, 1); glVertex3f(1280,480,0);
      glTexCoord2f(0, 1); glVertex3f(640,480,0);
      glEnd();
      
      
      if(lockFlag)
      {
      glBindTexture(GL_TEXTURE_2D, gl_depth_lock_tex);
      glTexImage2D(GL_TEXTURE_2D, 0, 1, 640, 480, 0, GL_LUMINANCE, GL_UNSIGNED_SHORT, &depthLockPtr[0]);	
      
      glBegin(GL_TRIANGLE_FAN);
      glColor4f(255.0f, 255.0f, 255.0f, 255.0f);
      glTexCoord2f(0, 0); glVertex3f(0,480,0);
      glTexCoord2f(1, 0); glVertex3f(640,480,0);
      glTexCoord2f(1, 1); glVertex3f(640,960,0);
      glTexCoord2f(0, 1); glVertex3f(0,960,0);
      glEnd();
      
      glBindTexture(GL_TEXTURE_2D, gl_rgb_lock_tex);
      glTexImage2D(GL_TEXTURE_2D, 0, 3, 640, 480, 0, GL_RGB, GL_UNSIGNED_BYTE, &rgbLockPtr[0]);
      
      glBegin(GL_TRIANGLE_FAN);
      glColor4f(255.0f, 255.0f, 255.0f, 255.0f);
      glTexCoord2f(0, 0); glVertex3f(640,480,0);
      glTexCoord2f(1, 0); glVertex3f(1280,480,0);
      glTexCoord2f(1, 1); glVertex3f(1280,960,0);
      glTexCoord2f(0, 1); glVertex3f(640,960,0);
      glEnd();
      }
   
      glutSwapBuffers();
   
      if(checkTime() && startFlag)
      {
         lockFlag = true;
         e = writeToBinaryFile(&rgbPtr, &depthPtr, i);
         e = kinectController->loadLockImages(&rgbLockPtr, &depthLockPtr);
         t = 0;
         i++;
         if(i == 16)
         {
            cout << "\nDone scanning\n" <<endl;
            startFlag = false;
            i=1;
         }
      }   
   }
   
   void key(unsigned char key, int x, int y)
   {
      if (key == 27) {
         glutDestroyWindow(window);
      }
      if (key == '1') {
         kinectController->setLed(LED_GREEN);
      }
      if (key == '2') {
         kinectController->setLed(LED_RED);
      }
      if (key == '3') {
         kinectController->setLed(LED_YELLOW);
      }
      if (key == '4') {
         kinectController->setLed(LED_BLINK_GREEN);
      }
      if (key == '5') {
         // 5 is the same as 4
         kinectController->setLed(LED_BLINK_GREEN);
      }
      if (key == '6') {
         kinectController->setLed(LED_BLINK_RED_YELLOW);
      }
      if (key == '0') {
         kinectController->setLed(LED_OFF);
      }
      
      if (key == 'f') {
         setStartFlagOn();
      }
      
      if (key == 'w') {
         freenect_angle++;
         if (freenect_angle > 30) {
            freenect_angle = 30;
         }
      }
      if (key == 's' || key == 'd') {
         freenect_angle = 0;
      }
      if (key == 'x') {
         freenect_angle--;
         if (freenect_angle < -30) {
            freenect_angle = -30;
         }
      }
      if (key == 'e') {
         freenect_angle = 10;
      }
      if (key == 'c') {
         freenect_angle = -10;
      }
      kinectController->setTiltDegrees(freenect_angle);
   }
   
   void resize(int Width, int Height)
   {
      glViewport(0,0,Width,Height);
      glMatrixMode(GL_PROJECTION);
      glLoadIdentity();
      glOrtho (0, 1280, 960, 0, -1.0f, 1.0f);
      glMatrixMode(GL_MODELVIEW);
   }
   
   void InitGL(int Width, int Height)
   {
      glClearColor(0.0f, 0.0f, 0.0f, 0.0f);
      glClearDepth(1.0);
      glDepthFunc(GL_LESS);
      glDisable(GL_DEPTH_TEST);
      glEnable(GL_BLEND);
      glBlendFunc (GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
      glShadeModel(GL_SMOOTH);
      
      glGenTextures(1, &gl_depth_tex);
      glBindTexture(GL_TEXTURE_2D, gl_depth_tex);
      glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR);
      glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR);
      glGenTextures(1, &gl_rgb_tex);
      glBindTexture(GL_TEXTURE_2D, gl_rgb_tex);
      glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR);
      glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR);
      
      glGenTextures(1, &gl_depth_lock_tex);
      glBindTexture(GL_TEXTURE_2D, gl_depth_lock_tex);
      glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR);
      glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR);
      glGenTextures(1, &gl_rgb_lock_tex);
      glBindTexture(GL_TEXTURE_2D, gl_rgb_lock_tex);
      glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR);
      glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR);
      //ReSizeGLScene(Width, Height);
      outerGlutReshapeFunc(Width, Height);
   }
};
	
MasterController device;


void outerGlutDisplayFunc()
{
   device.draw();
}

void outerglutIdleFunc()
{
   device.draw();
}

void outerGlutReshapeFunc(int Width, int Height)
{
   device.resize(Width, Height);
}

void outerGlutKeyboardFunc(unsigned char key, int x, int y)
{
   device.key(key, x, y);
}



int main(int argc, char **argv)
{	
   char eingabe = '0';

   std::cout << "rotating 3D scanner \n" << std::endl;

   device.init();
   device.start();
   
   device.kinectController->stopVideo();
   device.kinectController->stopDepth();

   return 0;
}